/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: Speed Controller Interface
 *         $Id: speed.c 5 2013-12-18 04:48:03Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <string.h>
#include "maxq_config.h"
#include "comm/maxq_comm.h"
#include "car.h"
#include "speed.h"
#include "encoder.h"

/***** Constants *****/
#if 0
#define KP  5   // multiply amount
#define KI  16  // divide amount
#define KD  3   // multiply amount
#else
#define KP  8   // multiply amount
#if 0
#define KI  32  // divide amount
#else
#define KI  0  // divide amount
#endif
#define KD  5   // multiply amount
#endif

#define IMAX    1024
#define IMIN    -1024

// Speed Controller States
#define SPD_ZERO      0
#define SPD_STARTUP   1
#define SPD_CHANGE    2
#define SPD_STABLE    3

/***** Global Data *****/
static unsigned int state[NUM_WHEELS];
unsigned int prev_setting[NUM_WHEELS];
static int prev_error[NUM_WHEELS];
static int prev_error_valid[NUM_WHEELS];
static int cumulative_error[NUM_WHEELS];

#if (SPEED_LOG_SIZE > 0)
#define SPEED_LOG_WHEEL   LEFT
typedef struct {
  unsigned int state;
  unsigned int setpoint;
  unsigned int speed;
  int ekp;
  int eki;
  int ekd;
  unsigned int setting;
} speed_ctrl_log_t;
speed_ctrl_log_t speed_ctrl_log[NUM_WHEELS][SPEED_LOG_SIZE];
unsigned int speed_ctrl_logi[NUM_WHEELS];
#endif

/***** Function Prototypes *****/


/****************************************************************************/
uint16_t sadd16(uint16_t a, int16_t b)
{
  unsigned int sum;

  sum = a + b;

  if (b > 0) {
    if (sum < a)
      sum = 0xFFFF;
  }
  else if (sum > a) {
    sum = 0x0000;
  }

  return sum;
}

/****************************************************************************/
void speed_ctrl_init(void)
{
  speed_ctrl_clear(LEFT, 0);
  speed_ctrl_clear(RIGHT, 0);
  state[LEFT] = SPD_ZERO;
  state[RIGHT] = SPD_ZERO;

#if (SPEED_LOG_SIZE > 0)
  speed_log_clear();
#endif
}

/****************************************************************************/
void speed_ctrl_clear(unsigned int wheel, uint8_t init_value)
{
  prev_setting[wheel] = init_value << 8;
  prev_error[wheel] = 0;
  prev_error_valid[wheel] = 0;
  cumulative_error[wheel] = 0;
}

/****************************************************************************/
#if (SPEED_LOG_SIZE > 0)
void speed_log_clear(void)
{
  memset(speed_ctrl_log, 0, sizeof(speed_ctrl_log));
  speed_ctrl_logi[LEFT] = 0;
  speed_ctrl_logi[RIGHT] = 0;
}
#endif

volatile int temperr, tempspd;
/****************************************************************************/
uint8_t speed_ctrl(unsigned int wheel, unsigned int speed, unsigned int setpoint)
{
  unsigned int new_setting;
  int ekp = 0;
  int eki = 0;
  int ekd = 0;
  int error, derivative_error;
  static unsigned int pre_setpoint[NUM_WHEELS] = { 0, 0 };
  static unsigned int stable_cnt[NUM_WHEELS] = { 0, 0 };

  // Handle zero speed request
  if (setpoint == 0) {
    speed_ctrl_clear(wheel, 0);
    if (speed == 0)
      state[wheel] = SPD_ZERO;
    else
      state[wheel] = SPD_CHANGE;
    return 0;
  }

  // Calculate error
  error = (int)setpoint - (int)speed;

  // Update controller state
  if ( (state[wheel] == SPD_ZERO) && (setpoint != 0) )
    state[wheel] = SPD_STARTUP;
  else if ( (state[wheel] == SPD_STARTUP) && enc_is_speed_valid() ) {
    state[wheel] = SPD_CHANGE;
  }
  else if (setpoint != pre_setpoint[wheel]) {
    state[wheel] = SPD_CHANGE;
    stable_cnt[wheel] = 0;
    prev_error_valid[wheel] = 0;
  } else if (state[wheel] == SPD_CHANGE) {
    if ( (error > -10) && (error < 10) )
      stable_cnt[wheel]++;
    else
      stable_cnt[wheel] = 0;

    if (stable_cnt[wheel] > 2)
      state[wheel] = SPD_STABLE;
  }

#if 0
  if (state[wheel] == SPD_STARTUP) {
    new_setting = ( (wheel == LEFT) ? (START_VALUE_LEFT << 8) : (START_VALUE_RIGHT << 8) );
  }
  else {
#else
  {
#endif
    // Calculate the derivative error
    if (prev_error_valid[wheel])
      derivative_error = error - prev_error[wheel];
    else
      derivative_error = 0;

    // Calculate the integral error
    cumulative_error[wheel] += error;
  
    // Saturate the integral error
    if (cumulative_error[wheel] > IMAX)
      cumulative_error[wheel] = IMAX;
    else if (cumulative_error[wheel] < IMIN)
      cumulative_error[wheel] = IMIN;
  
    // Calculate the PID terms
    ekp = KP * error;
#if (KI > 0)
    eki = cumulative_error[wheel] / KI;
#else
    eki = 0;
#endif
    ekd = KD * derivative_error;
  
    // Calculate the new controller output
    new_setting = sadd16(prev_setting[wheel], ekd);
    new_setting = sadd16(new_setting, eki);
    new_setting = sadd16(new_setting, ekp);
  }

  // Record state for next iteration
  prev_error[wheel] = error;
  prev_error_valid[wheel] = 1;
  prev_setting[wheel] = new_setting;
  pre_setpoint[wheel] = setpoint;

#if (SPEED_LOG_SIZE > 0)
  speed_ctrl_log[wheel][speed_ctrl_logi[wheel]].state = state[wheel];
  speed_ctrl_log[wheel][speed_ctrl_logi[wheel]].setpoint = setpoint;
  speed_ctrl_log[wheel][speed_ctrl_logi[wheel]].speed = speed;
  speed_ctrl_log[wheel][speed_ctrl_logi[wheel]].setting = new_setting;
  speed_ctrl_log[wheel][speed_ctrl_logi[wheel]].ekp = ekp;
  speed_ctrl_log[wheel][speed_ctrl_logi[wheel]].eki = eki;
  speed_ctrl_log[wheel][speed_ctrl_logi[wheel]].ekd = ekd;
  
  if (++speed_ctrl_logi[wheel] >= SPEED_LOG_SIZE)
    speed_ctrl_logi[wheel] = 0;
#endif

  // Round and scale return value
  new_setting = sadd16(new_setting, 0x80);  // add for rounding
  new_setting >>= 8;

  return new_setting;
}

#if (SPEED_LOG_SIZE > 0)
/****************************************************************************/
void speed_log_display(void)
{
  unsigned int i, wheel, timestamp;

  maxq_printf(C"\r\n***** Speed Log *****\r\n");
  maxq_printf(" Time| St Speed   Err St Speed   Err  Diff|   EKp   EKi   EKd  Outp  |   EKp   EKi   EKd  Outp\r\n");

  timestamp = SPEED_LOG_SIZE * 100;
  for (i = speed_ctrl_logi[LEFT]; i < SPEED_LOG_SIZE; i++) {
    timestamp -= 100;
    maxq_printf("%5u|", timestamp);
    maxq_printf(" %2d", speed_ctrl_log[LEFT][i].state);
    maxq_printf(" %5d %5d", speed_ctrl_log[LEFT][i].speed, (int)speed_ctrl_log[LEFT][i].setpoint - (int)speed_ctrl_log[LEFT][i].speed);
    maxq_printf(" %2d", speed_ctrl_log[RIGHT][i].state);
    maxq_printf(" %5d %5d", speed_ctrl_log[RIGHT][i].speed, (int)speed_ctrl_log[RIGHT][i].setpoint - (int)speed_ctrl_log[RIGHT][i].speed);
    maxq_printf(" %5d", speed_ctrl_log[LEFT][i].speed - speed_ctrl_log[RIGHT][i].speed);

    for (wheel = 0; wheel < NUM_WHEELS; wheel++) {
      maxq_printf("| %5d", speed_ctrl_log[wheel][i].ekp);
      maxq_printf(" %5d", speed_ctrl_log[wheel][i].eki);
      maxq_printf(" %5d", speed_ctrl_log[wheel][i].ekd);
      maxq_printf(" %04x", speed_ctrl_log[wheel][i].setting);
      maxq_printf(" %02x", sadd16(speed_ctrl_log[wheel][i].setting, 0x80) >> 8);
    }

    maxq_printf("\r\n");
  }

  for (i = 0; i < speed_ctrl_logi[LEFT]; i++) {
    timestamp -= 100;
    maxq_printf("%5u|", timestamp);
    maxq_printf(" %2d", speed_ctrl_log[LEFT][i].state);
    maxq_printf(" %5d %5d", speed_ctrl_log[LEFT][i].speed, (int)speed_ctrl_log[LEFT][i].setpoint - (int)speed_ctrl_log[LEFT][i].speed);
    maxq_printf(" %2d", speed_ctrl_log[RIGHT][i].state);
    maxq_printf(" %5d %5d", speed_ctrl_log[RIGHT][i].speed, (int)speed_ctrl_log[RIGHT][i].setpoint - (int)speed_ctrl_log[RIGHT][i].speed);
    maxq_printf(" %5d", speed_ctrl_log[LEFT][i].speed - speed_ctrl_log[RIGHT][i].speed);

    for (wheel = 0; wheel < NUM_WHEELS; wheel++) {
      maxq_printf("| %5d", speed_ctrl_log[wheel][i].ekp);
      maxq_printf(" %5d", speed_ctrl_log[wheel][i].eki);
      maxq_printf(" %5d", speed_ctrl_log[wheel][i].ekd);
      maxq_printf(" %04x", speed_ctrl_log[wheel][i].setting);
      maxq_printf(" %02x", sadd16(speed_ctrl_log[wheel][i].setting, 0x80) >> 8);
    }

    maxq_printf("\r\n");
  }
}
#endif
